#include "hal_errorcode.h"
#include <cfg_param.h>
#include <string.h>

static union MotorErrorCodeUn MotorErrotCodeGroup[MOTOR_MAX_ERROR_CHANNEL] = {0};
static union LaserErrorCodeUn LaserErrorCode[LASER_MAX_ERROR_CHANNEL]      = {0};
static uint32_t SystemErrorCode                                      = 0;

void hal_ErrorCodeWrite(ErrorCodeTypeEn ErrorCodeType, uint8_t code_form, uint8_t channel, uint8_t code)
{
    switch (ErrorCodeType)
    {
    case ERRORCODE_TYPE_MOTOR:
        // 电衰错误码
        if (channel > 0 && channel <= MOTOR_MAX_ERROR_CHANNEL)
        {
            MotorErrotCodeGroup[channel - 1].all_bit &= ~(1 << code_form);
            MotorErrotCodeGroup[channel - 1].all_bit |= (code << code_form);
        }
        break;
    case ERRORCODE_TYPE_LASER:
        // 激光错误码
        if (channel > 0 && channel <= LASER_MAX_ERROR_CHANNEL)
        {
            LaserErrorCode[channel - 1].all_bit &= ~(1 << code_form);
            LaserErrorCode[channel - 1].all_bit |= (code << code_form);
        }
        break;
    case ERRORCODE_TYPE_COM_SYSTEM:
        // 系统公用错误码
        SystemErrorCode &= ~(1 << code_form);
        SystemErrorCode |= (code << code_form);
        break;
    default:
        break;
    }
}

void hal_ErrorcodeRead(ErrorCodeTypeEn ErrorCodeType, uint8_t channel, uint32_t *ErrorCodeFinal)
{
    switch (ErrorCodeType)
    {
    case ERRORCODE_TYPE_MOTOR:
        // 返回电衰错误码和系统公用错误码
        MotorErrotCodeGroup[channel - 1].all_bit |= SystemErrorCode;
        *ErrorCodeFinal = MotorErrotCodeGroup[channel - 1].all_bit;
        break;
    case ERRORCODE_TYPE_LASER:
        // 返回激光错误码和系统公用错误码
        LaserErrorCode[channel - 1].all_bit |= SystemErrorCode;
        *ErrorCodeFinal = LaserErrorCode[channel - 1].all_bit;
        break;
    default:
        break;
    }
}

void hal_ErrorcodeReset()
{
    // 电衰/激光错误码和系统公用错误码清零
    SystemErrorCode = 0;

    memset(MotorErrotCodeGroup, 0, sizeof(MotorErrotCodeGroup));

    memset(LaserErrorCode, 0, sizeof(LaserErrorCode));
}
